#include "OpenNIContextManager.h"


OpenNIContextManager::OpenNIContextManager ():
		_deviceIter (_deviceInfoList.Begin ())
{
	// 1. initial context
	_result = _context.Init ();
	_checkOpenNIError ("OpenNIContextManager::OpenNIContextManager(): "
			"Fail to initialize OpenNI context");

	// 2. Enumerate Device
	_result = _context.EnumerateProductionTrees (XN_NODE_TYPE_DEVICE, NULL,
			_deviceInfoList);
	_checkOpenNIError ("OpenNIContextManager::OpenNIContextManager(): "
			"Fail to enumerate devices");

	// 3. check if there is device existed
	if (_deviceInfoList.IsEmpty())
		throw std::exception ("OpenNIContextManager::OpenNIContextManager(): "
				"No devices found");
	_deviceIter = _deviceInfoList.Begin ();
}


OpenNIContextManager::~OpenNIContextManager ()
{
	_context.Release ();
}

SensorDataKinectInput *OpenNIContextManager::createSensor (
		const std::string &name, const std::string &intrinsicFile,
		const std::string &extrinsicFile)
{
	// check if there is already Device window existed
	if (_deviceIter == _deviceInfoList.End ())
		return NULL;

	xn::NodeInfo nodeInfo = *_deviceIter;
	SensorDataKinectInput *pSensor = new SensorDataKinectInput (name,
			intrinsicFile, extrinsicFile);
	// 1. create and initial OpenNI nodes
	try {
		// a. create device
		_result = _context.CreateProductionTree (nodeInfo,
				pSensor->_device);
		_checkOpenNIError ("OpenNIContextManager::createSensor(): "
				"Fail to create device");

		// b. build query rule for given device
		xn::Query query;
		query.AddNeededNode (nodeInfo.GetInstanceName ());

		// c. create depth generator
		_result = pSensor->_depth.Create (_context, &query);
		_checkOpenNIError ("OpenNIContextManager::createSensor(): "
				"Fail to create depth generator");

		// d. create image generator
		_result = pSensor->_image.Create (_context, &query);
		_checkOpenNIError ("OpenNIContextManager::createSensor(): "
				"Fail to create image generator");

		pSensor->initial ();
	} catch (...) {
		delete pSensor;
		throw;
	}

	++_deviceIter;
	return pSensor;
}

void OpenNIContextManager::_checkOpenNIError (const std::string& info)
{
	if (_result != XN_STATUS_OK)
		throw std::exception (info.c_str ());
}

void OpenNIContextManager::start ()
{
	_result = _context.StartGeneratingAll ();
	_checkOpenNIError ("OpenNIContextManager::start()"
			"Fail to start OpenNI");
}

void OpenNIContextManager::update ()
{
	_result = _context.WaitAndUpdateAll ();
	_checkOpenNIError ("OpenNIContextManager::update()"
			"Fail to update all devices");
}
